﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using Drifting;
using Drifting.MotionPlanning.Primitives;
using Drifting.MotionPlanning.Systems;
using Drifting.MotionPlanning.PointSets;
using Drifting.Foundation;

namespace Drifting.MotionPlanning.SingleThreading.Algorithms
{
    public class SST : TreeBasedPlanningAlgorithm
    {
        protected PointSet S;
        protected PointSet Vactive;
        protected double dv;
        protected double ds;
        protected int _measurement_selection_bestNear;
        protected int _measurement_select_nearest;

        public SST(BaseSystem system, Random rand, double dv, double ds)
            : base (system, rand)
        {
            int[] branches = new int[]{6,6,6,6,6,6};
            Vactive = new BinQuery(system.Min_Alias, system.Max_Alias, system.DOF_Alias, branches);
            S = new BinQuery(system.Min_Alias, system.Max_Alias, system.DOF_Alias, branches);
            
            SetRoot(system.GenerateZeroState());

            this.dv = dv;
            this.ds = ds;
        }

        public SST(BaseSystem system, Random rand, string initializations)
            : base(system, rand)
        {
            string[] separators = null;
            string[] entries = null;

            separators = new string[] { "<R_v>", "</R_v>" };
            entries = initializations.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) throw new ArgumentException();
            this.dv = double.Parse(entries[1]);

            separators = new string[] { "<R_s>", "</R_s>" };
            entries = initializations.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) throw new ArgumentException();
            this.ds = double.Parse(entries[1]);

            separators = new string[] { "<BinQuery_Branches>", "</BinQuery_Branches>" };
            entries = initializations.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) throw new ArgumentException();
            int[] branches = TextAssistant.StringToIntArray(entries[1]);

            Vactive = new BinQuery(system.Min_Alias, system.Max_Alias, system.DOF_Alias, branches);
            S = new BinQuery(system.Min_Alias, system.Max_Alias, system.DOF_Alias, branches);

            SetRoot(system.GenerateZeroState());
        }

        override public PointSet Vertices
        {
            get { return this.Vactive; }
            set { this.Vactive = value; }
        }

        override public void SetRoot(double[] q_root)
        {
            x_root = SpawnState(q_root);
            Vactive.AddState(x_root);

            Representation s_root = new Representation(x_root.Alias, x_root);
            S.AddState(s_root);
        }

        override public void Execute(object input)
        {
            if (input is Int32 == false)
                throw new ArgumentException();
            n_exec = (Int32)input;
            if (x_root == null)
                throw new InvalidOperationException("Forget to call SetRoot first?");

            _measurement_selection_bestNear = 1;
            _measurement_select_nearest = 1;

            n_current = 0;
            double[] a_sample = null;
            List<State> neighbors = null;
            while (n_current < n_exec)
            {
                n_current++;

                //Sample
                system.SampleAlias(rand, ref a_sample);

                //Neighbor selection
                State x_nearest = null;
                double dist2_sample = 0;
                Vactive.Neighbor_RangeQuery(a_sample, dv, ref neighbors);
                if (neighbors != null && neighbors.Count > 0)
                {
                    x_nearest = BestNearWithin(neighbors);
                    _measurement_selection_bestNear++;
                }
                else
                {
                    Vactive.Neighbor_SingleQuery(a_sample, ref x_nearest, ref dist2_sample);
                    _measurement_select_nearest++;
                }

                //Propagate
                double[] iCtrl = null;
                double iDt = double.MaxValue;
                system.SampleControl(rand, ref iCtrl, ref iDt);

                double[] q_new = null;
                double[] a_new = null;
                double iCost = double.MaxValue;
                bool propagate_success = system.Propagate(memoryPad, x_nearest.Q, iCtrl, iDt, ref q_new, ref iCost);
                AliasOf(q_new, ref a_new);

                if (propagate_success)
                {
                    S.Neighbor_RangeQuery(a_new, ds, ref neighbors);
                    Representation s_new = null;
                    State x_new = null;
                    Edge e_new = null;
                    double cost_new = x_nearest.Cost + iCost;
                    if(neighbors.Count == 0)
                    {
                        //Grow G
                        x_new = SpawnState(q_new, a_new);
                        e_new = SpawnEdge(x_nearest, x_new, iCtrl, iDt, iCost);
                        x_new.ParentEdge = e_new;
                        x_new.Cost = cost_new;
                        Vactive.AddState(x_new);
                        // add new s
                        s_new = new Representation(a_new, x_new);
                        S.AddState(s_new);
                    }
                    else
                    {
                        // get the closet s
                        Representation s_closest = null;
                        double dist2_clostest = double.MaxValue;
                        foreach (Representation s_i in neighbors)
                        {
                            double[] a_s = s_i.Alias;
                            double dist2_i = PointSet.Euclidean2(a_new, a_s);
                            if (dist2_clostest >= dist2_i)
                            {
                                dist2_clostest = dist2_i;
                                s_closest = s_i;
                            }
                        }

                        // peer compare
                        State x_peer = s_closest.X;
                        if (x_peer.Cost >= cost_new)
                        {
                            // grow G
                            x_new = SpawnState(q_new, a_new);
                            e_new = SpawnEdge(x_nearest, x_new, iCtrl, iDt, iCost);
                            x_new.ParentEdge = e_new;
                            x_new.Cost = cost_new;
                            Vactive.AddState(x_new);

                            // replace the representitive
                            s_closest.X = x_new;

                            // delete x_peer from Vactive
                            Vactive.DelState(x_peer);
                        }
                    }
                }

                //Auxillary
                //Thread.Sleep(3);
            }
        }

        protected State BestNearWithin(List<State> neighbors)
        {
            State x_best = null;
            double cost_min = double.MaxValue;
            foreach (State x_i in neighbors)
            {
                if (cost_min >= x_i.Cost)
                {
                    cost_min = x_i.Cost;
                    x_best = x_i;
                }
            }
            return x_best;
        }
        
        override public void ReportStatistics(ref List<string> info)
        {
            if (info.Count < 1) info.Add(null);
            double percent = Math.Floor(n_current / (double)n_exec * 10000);
            int Z = (int)(percent / 100);
            int D = (int)(percent % 100);

            sb.Clear();
            sb.Append("progress = ").Append(Z);
            if (D == 0)
                sb.Append(".00");
            else if (D < 10)
                sb.Append(".0").Append(D);
            else
                sb.Append(".").Append(D);
            sb.Append("% = ").Append(n_current).Append(" / ").Append(n_exec);
            info[0] = sb.ToString();

            if (info.Count < 2) info.Add(null);
            double ratio = _measurement_selection_bestNear / (double)_measurement_select_nearest;
            string ratio_readable = null;
            if (ratio < 1)
                ratio_readable = "1 : " + (int)(1 / ratio);
            else
                ratio_readable = ((int)ratio) + " : 1";

            sb.Clear();
            sb.Append("bestnear vs nearest = ").Append(ratio_readable);
            info[1] = sb.ToString();
        }

        override public void AliasOf(double[] q, ref double[] alias)
        {
            if (alias == null)
                alias = new double[system.DOF_Alias];
            for (int i = 0; i < system.DOF_Alias; ++i)
                alias[i] = q[i];
        }
    }
}
